#ifndef KF_H
#define KF_H
#include <Eigen/Dense>
/**
 * @brief The Kalman Filter class
 *
 */

typedef Eigen::MatrixXd MatrixXd;
/**
 * @brief The SysModel class,系统模型，此处默认是常加速度模型（CA），无控制输入。
 */
class SysModel
{
public:
    SysModel();
    SysModel(MatrixXd state, MatrixXd stateCov, MatrixXd q);

    virtual ~SysModel();
    /**
     * @brief f,状态转移函数，从当前时刻状态计算下一时刻状态。
     * @param delteT,下一时刻与当前时刻在时间差
     * @return,下一时刻的状态。
     */
    virtual MatrixXd f(double delteT);


    /**
     * @brief Q 计算系统模型噪声
     * @param delteT,时间间隔。
     * @return 系统模型噪声矩阵。
     */
    virtual MatrixXd Q(double delteT);

    /**
     * @brief state
     * @return 系统状态。
     */
    MatrixXd& state();
    /**
     * @brief stateCov
     * @return 系统状态协方差。
     */
    MatrixXd& stateCov();
    /**
     * @brief q，在卡尔曼滤波公式中的q。
     * @return 模型噪声协方差。
     */
    MatrixXd& q();
private:
    /**
     * @brief mState：系统状态，mStateCov：系统噪声协方差，mSysCov：系统模型噪声协方差。
     */
    MatrixXd mState,mStateCov,mq;

};

class MeasureModel
{
public:
    MeasureModel();
    virtual ~MeasureModel();
    MeasureModel(MatrixXd R);

    /**
     * @brief R,量测噪声协方差。
     * @return mR的引用。
     */
    MatrixXd& R();

    /**
     * @brief h,量测过程，此处只观测到位置。
     * @return 量测矩阵
     */
    virtual MatrixXd h();

private:
    /**
     * @brief mMeasureCov,量测噪声
     */
    MatrixXd mR;
};

class KF
{
public:
    /**
     * @brief KF
     * @param 同set()
     */
    KF(SysModel sys, MeasureModel measure);

    virtual ~KF(){}


    /**
     * @brief 加入一个新的量测值
     * @param z 量测点的状态
     * @param t 量测值产生的时间
     */
    virtual void addZk(MatrixXd z, double delteT);

    /**
     * @brief sysState,返回系统模型，包含状态向量，噪声协方差，系统模型噪声协方差信息。
     * @return 系统模型对象的引用。
     */
    SysModel& sysModel()
    {
        return mSys;
    }

    /**
     * @brief measureModel,返回量测模型。
     * @return mMeasure的引用。
     */
    MeasureModel& measureModel()
    {
        return mMeasure;
    }


private:
    SysModel mSys;
    MeasureModel mMeasure;
    MatrixXd mGammak;
};

#endif // KF_H
